package star.crust.cartspace.framework;

import java.util.ArrayList;
import javax.vecmath.Matrix3f;
import star.core.util.geometry.FloatXYZ;

public class CrtObbUtil {

	private float a;
	private float[][] s;
	private float[] m;

	public CrtObbUtil(float a, float[][]s,float[] m ) {
		this.a=a;
		this.s=s;
		this.m=m;
	}

	public CrtObbUtil() {
		this.a = 0;
		this.s = new float[3][3];
		this.m = new float[3];
		this.clear_accum();
	}

	public void clear_accum() {
		this.m[0] = this.m[1] = this.m[2] = 0;
		this.s[0][0] = this.s[0][1] = this.s[0][2] = 0;
		this.s[1][0] = this.s[1][1] = this.s[1][2] = 0;
		this.s[2][0] = this.s[2][1] = this.s[2][2] = 0;
		this.a = 0;
	}

	public void accum_moment(CrtObbUtil b) {
		this.m[0] += b.m[0] * b.a;
		this.m[1] += b.m[1] * b.a;
		this.m[2] += b.m[2] * b.a;

		this.s[0][0] += b.s[0][0];
		this.s[0][1] += b.s[0][1];
		this.s[0][2] += b.s[0][2];
		this.s[1][0] += b.s[1][0];
		this.s[1][1] += b.s[1][1];
		this.s[1][2] += b.s[1][2];
		this.s[2][0] += b.s[2][0];
		this.s[2][1] += b.s[2][1];
		this.s[2][2] += b.s[2][2];

		this.a += b.a;
	}

	public Matrix3f covariance_from_accum(Matrix3f C) {
		int i,j;
		for(i=0; i<3; i++) for(j=0; j<3; j++) C.setElement(i, j, this.s[i][j] - this.m[i]*this.m[j]/this.a );
		return C;
	}

	public float[] mean_from_moment() {
		float[] M = new float[3];
		M[0] = this.m[0];
		M[1] = this.m[1];
		M[2] = this.m[2];
		return M;
	}

	public float[] mean_from_accum() {
		float[] M = new float[3];
		M[0] = this.m[0] / this.a;
		M[1] = this.m[1] / this.a;
		M[2] = this.m[2] / this.a;
		return M;
	}

	/**
	 * Metodi per il calcolo dei momenti che
	 * saranno utilizzati per la creazione della
	 * gerarchia OBB
	 * @param M
	 * @param i
	 * @param p
	 * @param q
	 * @param r
	 * @return
	 */
	private static ArrayList<CrtObbUtil> compute_moment(ArrayList<CrtObbUtil> M, int i, FloatXYZ p, FloatXYZ q, FloatXYZ r) {
		CrtObbUtil m = new CrtObbUtil();
		float[] p1 = CrtObbMath.FloatXYZ2Vett(p);
		float[] q1 = CrtObbMath.FloatXYZ2Vett(q);
		float[] r1 = CrtObbMath.FloatXYZ2Vett(r);

		float[] u = {0,0,0};
		float[] v = {0,0,0};
		float[] w = {0,0,0};

		u = CrtObbMath.VmV(q1, p1);
		v = CrtObbMath.VmV(r1, p1);
		w = CrtObbMath.VcrossV(u, v);
		m.a = (float) 0.5 * CrtObbMath.Vlength(w);

		if (m.a == 0.0)
		{
			m.m[0] = (p1[0] + q1[0] + r1[0]) /3;
			m.m[1] = (p1[1] + q1[1] + r1[1]) /3;
			m.m[2] = (p1[2] + q1[2] + r1[2]) /3;

			m.s[0][0] = (p1[0]*p1[0] + q1[0]*q1[0] + r1[0]*r1[0]);
			m.s[0][1] = (p1[0]*p1[1] + q1[0]*q1[1] + r1[0]*r1[1]);
			m.s[0][2] = (p1[0]*p1[2] + q1[0]*q1[2] + r1[0]*r1[2]);
			m.s[1][1] = (p1[1]*p1[1] + q1[1]*q1[1] + r1[1]*r1[1]);
			m.s[1][2] = (p1[1]*p1[2] + q1[1]*q1[2] + r1[1]*r1[2]);
			m.s[2][2] = (p1[2]*p1[2] + q1[2]*q1[2] + r1[2]*r1[2]);      
			m.s[2][1] = m.s[1][2];
			m.s[1][0] = m.s[0][1];
			m.s[2][0] = m.s[0][2];

			M.add(i, m); 
			return M;
		}

		m.m[0] = (p1[0] + q1[0] + r1[0])/3;
		m.m[1] = (p1[1] + q1[1] + r1[1])/3;
		m.m[2] = (p1[2] + q1[2] + r1[2])/3;

		m.s[0][0] = m.a*(9*m.m[0]*m.m[0]+p1[0]*p1[0]+q1[0]*q1[0]+r1[0]*r1[0])/12;
		m.s[0][1] = m.a*(9*m.m[0]*m.m[1]+p1[0]*p1[1]+q1[0]*q1[1]+r1[0]*r1[1])/12;
		m.s[1][1] = m.a*(9*m.m[1]*m.m[1]+p1[1]*p1[1]+q1[1]*q1[1]+r1[1]*r1[1])/12;
		m.s[0][2] = m.a*(9*m.m[0]*m.m[2]+p1[0]*p1[2]+q1[0]*q1[2]+r1[0]*r1[2])/12;
		m.s[1][2] = m.a*(9*m.m[1]*m.m[2]+p1[1]*p1[2]+q1[1]*q1[2]+r1[1]*r1[2])/12;
		m.s[2][2] = m.a*(9*m.m[2]*m.m[2]+p1[2]*p1[2]+q1[2]*q1[2]+r1[2]*r1[2])/12;
		m.s[2][1] = m.s[1][2];
		m.s[1][0] = m.s[0][1];
		m.s[2][0] = m.s[0][2];

		M.add(i, m); 
		return M;
	}

	public static ArrayList<CrtObbUtil> compute_moments(ArrayList<CrtObbUtil> M, ArrayList<CrtMeshPrimitive> tris) {
		int i;

		float Amin = 0;
		boolean zero = false;
		for (i = 0; i < tris.size(); i++) {
			compute_moment(M, i, tris.get(i).getVertex(0).position(), tris.get(i).getVertex(1).position(), tris.get(i).getVertex(2).position());
			if (M.get(i).a == 0.0) zero = true;
			else {
				if (Amin == 0.0) Amin = M.get(i).a;
				else if (M.get(i).a < Amin) Amin = M.get(i).a;
			}
		}

		if (zero) {
			if (Amin == 0.0) Amin = 1;
			for (i = 0; i < tris.size(); i++) if (M.get(i).a == 0) M.get(i).a = Amin;
		}
		return M;
	}

	void reaccum_moments(int[] t, int start, int n, ArrayList<CrtObbUtil> RAPID_moment)
	{
		this.clear_accum();
		for(int i=start; i<start+n; i++) this.accum_moment(RAPID_moment.get(t[i]));
	}

	public String toString() {
		return "\nA: "+this.a+"\nm: "+this.m[0]+" "+this.m[1]+" "+this.m[2]+"\ns: "+this.s[0][0]+" "+this.s[0][1]+" "+this.s[0][2]+"\n"+this.s[1][0]+" "+this.s[1][1]+" "+this.s[1][2]+"\n"+this.s[2][0]+" "+this.s[2][1]+" "+this.s[2][2];
	}
}
